%% Input and outputs
% Input
% 1. Ws: Stable manifold cell
% 2. Wu: UnstableManifold cell
% 3. Plane location
%     a. PrimaryBody
%     b. SecondaryBody
% 4. plane rotation angle
% 4. Use filter or not
% 5. Number of best trajectory sets
% output
% 1. Best trajectory sets
%     a. Trajectory information (Time and state)
%     b.
% 2. Best trajectory set information
%     a. DelV
%     b. DelJ
%     c. DelPhi

function output = Poincare(~,varargin)


%%
numvarargs = length(varargin);
% Number of optional arguments

if mod(numvarargs, 2) == 1
    error('Error: Please check name-value pair arguments');
end

name_arguments = varargin(1:2:end);
value_arguments = varargin(2:2:end);
% Initialize name-value arguments
figureBinary = 1;
for ii = 1:length(name_arguments)
    switch lower(name_arguments{ii})
        case 's_ws'
            Ws = value_arguments{ii};
        case 's_wu'
            Wu = value_arguments{ii};
        case 't_ws'
            t_Ws = value_arguments{ii};
        case 't_wu'
            t_Wu = value_arguments{ii};
        case 'planelocation'
            PlaneLocation = value_arguments{ii};
        case 'planerotationangle'
            POINCARE_PLANE_ROTATION = value_arguments{ii};
        case 'usefilter'
            UseFilter_Binary = value_arguments{ii};
        case 'besttrajectorynumber'
            NumbBestTrajectoryOutput = value_arguments{ii};
        case 'filteredtrajectorynumber'
            Best_of_Best = value_arguments{ii};
        case 'massratio'
            u = value_arguments{ii};
        case 'figure'
            figureBinary = value_arguments{ii};
    end
end

% PlaneLocation = 'secondarybody';
% POINCARE_PLANE_ROTATION = 50;
% u = 0.012;
%% Define Plane
%%%% WORK ON DEFINING PLANE %%%%


switch lower(PlaneLocation)

    case 'primarybody'
        T = Rotation([0;0;1],POINCARE_PLANE_ROTATION,'Degrees');    % Rotation matrix about z axis
        p1 = [0;0;0];           % Origin
        p2 = [1-u;0;0];           % position
        p3 = [0;0;-1];     % Point -1 below the position of Earth.

        p2_rot  = T*p2;
        Vec1    = p2_rot - p1;         % Vector from Earth to rotating point
        Vec2    = p3-p1;               % Vector from p1 to p3
        nhat    = cross(Vec1,Vec2);    % Perpendicular vector

        plane_eq = @(x,y,z) nhat(1)*(x-p1(1))+nhat(2)*(y)+nhat(3)*(z);  % Plane equation
        plane_eq_Draw = @(x,y) -(nhat(1)*(x-p1(1))+nhat(2)*(y))/0.001;
        %[] Define the poincare section.
    case 'secondarybody'
        T = Rotation([0;0;1],POINCARE_PLANE_ROTATION,'Degrees');    % Rotation matrix about z axis
        p1 = [1-u;0;0];      % Point at the position of Earth
        p2 = [2;0;0];           % Position in x axiseuropa
        p3 = [1-u;0;-1];     % Point -1 below the position of Earth.

        p2_temp = p2-p1;
        p2_temp = T*p2_temp;
        p2_temp = p2_temp+p1;
        Vec1 = p2_temp - p1;        % Vector from Earth to rotating point
        Vec2 = p3-p1;               % Vector from p1 to p3
        nhat = cross(Vec1,Vec2);    % Perpendicular vector
        %[] Define normal vector.

        plane_eq = @(x,y,z) nhat(1)*(x-p1(1))+nhat(2)*(y)+nhat(3)*(z);  % Plane equation
        plane_eq_Draw = @(x,y) -(nhat(1)*(x-p1(1))+nhat(2)*(y))/0.001;
        %[] Define the poincare section.
end



%%

%% Find the point where the state intersects

[row_WU,col_WU] = size(Wu);
poincare_WU = cell(row_WU,col_WU);
for i = 1:row_WU
    for j = 1:col_WU
        %[] For every row and column of the Moon...

        pointdata = plane_eq(Wu{i,j}(1,:),Wu{i,j}(2,:),Wu{i,j}(3,:));
        %[] Determine the signs with the plane equation

        ind_WU{i,j} = find(diff(sign(pointdata)));
        %[] Find the indices where the sign changes.

        for jj = 1:length(ind_WU{i,j})
            %[] For all the indices..

            a = Wu{i,j}(1:3,ind_WU{i,j}(jj))-p1;
            %[] Take out the point of rotation of interest.

            poincare_WU{i,j}(:,jj) = transpose(T)*a+p1;
            %[] Rotate and add the point of rotation again.

        end
    end
end

[row_WS,col_WS] = size(Ws);
poincare_WS = cell(row_WS,col_WS);
for i = 1:row_WS
    for j = 1:col_WS
        %[] For every row and column of the Moon...

        pointdata = plane_eq(Ws{i,j}(1,:),Ws{i,j}(2,:),Ws{i,j}(3,:));
        %[] Determine the signs with the plane equation

        ind_WS{i,j} = find(diff(sign(pointdata)));
        %[] Find the indices where the sign changes.

        for jj = 1:length(ind_WS{i,j})
            %[] For all the indices..

            a = Ws{i,j}(1:3,ind_WS{i,j}(jj))-p1;
            %[] Take out the point of rotation of interest.

            poincare_WS{i,j}(:,jj) = transpose(T)*a+p1;
            %[] Rotate and add the point of rotation again.

        end
    end
end
%% Plot poincar section
figure;
hold on;
poincare_WU_direction = [];
for i = 1:row_WU
    color = [rand, rand, rand];
    %[] Define an arbitrary color.

    for j = 1:col_WU
        %[] For all column of the Moon orbit

        n = size(poincare_WU{i,j});
        %[] Number of poincare points for each orbit

        for gg = 1:n(2)
            %[] For all the points...

            posorneg = plane_eq(Wu{i,j}(1,ind_WU{i,j}(gg)),...
                Wu{i,j}(2,ind_WU{i,j}(gg)),...
                Wu{i,j}(3,ind_WU{i,j}(gg)));
            %[] Determine the direction

            if posorneg > 0 % From positive
                plot(poincare_WU{i,j}(1,gg),poincare_WU{i,j}(3,gg),'+','Color',color...
                    ,'MarkerSize',5,'MarkerFaceColor',color)
                poincare_WU_direction = [poincare_WU_direction,1];

            else % From negative.
                plot(poincare_WU{i,j}(1,gg),poincare_WU{i,j}(3,gg),'o','Color',color...
                    ,'MarkerSize',5,'MarkerFaceColor',color)
                %[] Plot all the negatively piercing points.
                poincare_WU_direction = [poincare_WU_direction,-1];

            end
        end
    end
end
poincare_WS_direction = [];

for i = 1:row_WS
    for j = 1:col_WS
        n = size(poincare_WS{i,j});
        for gg = 1:n(2) % From positive
            posorneg = plane_eq(Ws{i,j}(1,ind_WS{i,j}(gg)),...
                Ws{i,j}(2,ind_WS{i,j}(gg)),...
                Ws{i,j}(3,ind_WS{i,j}(gg)));
            if posorneg > 0 % From positive
                plot(poincare_WS{i,j}(1,gg),poincare_WS{i,j}(3,gg),'r+','MarkerSize',5)
                poincare_WS_direction = [poincare_WS_direction,1];
            else % From negative
                plot(poincare_WS{i,j}(1,gg),poincare_WS{i,j}(3,gg),'ro','MarkerSize',3)
                poincare_WS_direction = [poincare_WS_direction,-1];

            end
        end
    end
end

xlabel('\Sigma_x');
ylabel('\Sigma_y');
ax = gca;
ax.FontSize = 30;
axis equal

%% Determine the best trajectory.

% Let's make them just a normal state set vector
%[] Make vector for vectorizing the cell data.
% VERY LONG ONE
poincare_WS_vec = [];
for jj = 1:row_WS
    for ii = 1:col_WS
        poincare_WS_vec = [poincare_WS_vec,poincare_WS{jj,ii}];
    end
end

% Not so long one.
% This is divided into cells in order to determine N number of trajectories with M number of halo orbits
poincare_WU_vec = cell(row_WU,1);
for jj = 1:row_WU
    for ii = 1:col_WU
        poincare_WU_vec{jj,1} = [poincare_WU_vec{jj,1},poincare_WU{jj,ii}];
    end
end



%[] Make a normal vector version
relDistMatrix = cell(row_WU,1);
for numb = 1:row_WU
    relDistMatrix{numb} = zeros(length(poincare_WS_vec),length(poincare_WU_vec{numb}));
end
parfor kk = 1:row_WU
    for ii = 1:length(poincare_WS_vec)
        for jj = 1:length(poincare_WU_vec{kk})
            relDist_3xn = poincare_WU_vec{kk}(:,jj)-poincare_WS_vec(:,ii);
            relDistMatrix{kk}(ii,jj) = norm(relDist_3xn);
        end
    end
end
for n = 1:row_WU
    for ii = 1:length(poincare_WS_direction)
        for jj = 1:length(poincare_WU_direction)
            if poincare_WS_direction(ii)+poincare_WU_direction(jj)~=0
                relDistMatrix{n}(ii,jj) = inf;
            end
        end
    end
end
% Fine N number of ref trajectories
for ii = 1:row_WU
    for jj = 1:NumbBestTrajectoryOutput
        [unstable_ind_temp,stable_ind_temp] = find(min(min(relDistMatrix{ii}))==relDistMatrix{ii});
        stable_ind{ii}(jj) = stable_ind_temp(1);
        unstable_ind{ii}(jj) = unstable_ind_temp(1);
        relDistMatrix{ii}(unstable_ind{ii}(jj),stable_ind{ii}(jj)) = inf;
    end
    plot(poincare_WS_vec(1,unstable_ind{ii}),poincare_WS_vec(3,unstable_ind{ii}),'md','MarkerFaceColor','k','MarkerSize',9)
    plot(poincare_WU_vec{ii}(1,stable_ind{ii}),poincare_WU_vec{ii}(3,stable_ind{ii}),'ms','MarkerFaceColor','k','MarkerSize',9)
end

%% Plot the output trajectories.

% From above, M number of cells, where the stored information is about the
% orbit of best poincare section analysis.


for ii = 1:length(stable_ind)
    % For all stable orbits 16 for now
    for jj = 1:length(stable_ind{ii})
        % For all the point of interest for 1 orbit
        % Find the unstable manifold
        countup = 0;
        countdesired = unstable_ind{ii}(jj);
        %Find that occurance's row and column
        for kk = 1:row_WS
            for nn = 1:col_WS
                [~,c] = size(poincare_WS{kk,nn});
                for indexcount = 1:c
                    countup = countup + 1;
                    if countup == countdesired
                        S_stable_Best_temp = Ws{kk,nn};
                        t_stable_Best_temp = t_Ws{kk,nn};
                        ind_stable_best_temp = ind_WS{kk,nn}(indexcount);
                        break
                    end
                end
            end
        end
        countup = 0;
        countdesired = stable_ind{ii}(jj);
        for kk = ii
            for nn = 1:col_WU
                [~,c] = size(poincare_WU{kk,nn});
                for indexcount = 1:c
                    countup = countup + 1;
                    if countup == countdesired
                        S_unstable_Best_temp = Wu{kk,nn};
                        t_unstable_Best_temp = t_Wu{kk,nn};
                        ind_unstable_best_temp = ind_WU{kk,nn}(indexcount);
                    end
                end
            end
        end

        S_unstable_Best{ii,jj} = S_unstable_Best_temp;
        t_unstable_Best{ii,jj} = t_unstable_Best_temp;
        ind_unstable_best{ii,jj} = ind_unstable_best_temp;
        S_stable_Best{ii,jj} = S_stable_Best_temp;
        t_stable_Best{ii,jj} = t_stable_Best_temp;
        ind_stable_best{ii,jj} = ind_stable_best_temp;
    end
end

%% Poincare Filter

[row_opt,col_opt] = size(ind_unstable_best);
angle = zeros(row_opt,col_opt);
mag_diff = zeros(row_opt,col_opt);
angle_diff = zeros(row_opt,col_opt);
dist_diff = zeros(row_opt,col_opt);

for ii = 1:row_opt
    for jj = 1:col_opt
        r1 = S_unstable_Best{ii,jj}(1:3,ind_unstable_best{ii,jj});
        r2 = S_stable_Best{ii,jj}(1:3,ind_stable_best{ii,jj});
        v1 = S_unstable_Best{ii,jj}(4:6,ind_unstable_best{ii,jj});
        v2 = S_stable_Best{ii,jj}(4:6,ind_stable_best{ii,jj});
        mag_diff(ii,jj)   = abs(norm(v1)-norm(v2));
        angle_diff(ii,jj) = acosd(dot(v1,v2)/(norm(v1)*norm(v2)));
        dist_diff(ii,jj) = norm(r1-r2);

    end
end

mag_diff_max = max(max(mag_diff));

angle_diff_max = max(max(angle_diff));
dist_diff_max = max(max(dist_diff));

mag_diff_norm = mag_diff/mag_diff_max;
angle_diff_norm = angle_diff/angle_diff_max;
dist_diff_norm = dist_diff/dist_diff_max;

overall_norm = 20*mag_diff_norm + 10*angle_diff_norm + 1*dist_diff_norm;

for ii = 1:Best_of_Best

    [a,b]=find(min(min(overall_norm))==overall_norm);

    overall_norm(a,b) = inf;

    S_unstable_Best_Best{ii} = S_unstable_Best{a,b};
    t_unstable_Best_Best{ii} = t_unstable_Best{a,b};
    S_stable_Best_Best{ii} = S_stable_Best{a,b};
    t_stable_Best_Best{ii} = t_stable_Best{a,b};
    ind_unstable_best_Best{ii} = ind_unstable_best{a,b};
    ind_stable_best_Best{ii} = ind_stable_best{a,b};
end

%% figure
OpenCRTBP_u(u);
% [X,Y] = meshgrid(-1.5:1.5);
% pic = plane_eq_Draw(X,Y);
% surface(X,Y,pic)
xlim([-1.5 1.5])
ylim([-1.5 1.5])
zlim([-1.5 1.5])
[r,c] = size(S_stable_Best);
for ii = 1:r
    for jj = 1:c
        p1 = plot3(S_unstable_Best{ii,jj}(1,1:ind_unstable_best{ii,jj}),...
            S_unstable_Best{ii,jj}(2,1:ind_unstable_best{ii,jj}),...
            S_unstable_Best{ii,jj}(3,1:ind_unstable_best{ii,jj}),...
            'Color','k');
        p1.Color(4) = 0.25;
        p2 = plot3(S_stable_Best{ii,jj}(1,1:ind_stable_best{ii,jj}),...
            S_stable_Best{ii,jj}(2,1:ind_stable_best{ii,jj}),...
            S_stable_Best{ii,jj}(3,1:ind_stable_best{ii,jj}),...
            'Color','k');
        p2.Color(4) = 0.25;
    end
end
xlim([-0.1 1.1])
ylim([-0.1 1.1])
for ii = 1:Best_of_Best
    colors = [rand,rand,rand];
    plot3(S_unstable_Best_Best{ii}(1,1:ind_unstable_best_Best{ii}),...
        S_unstable_Best_Best{ii}(2,1:ind_unstable_best_Best{ii}),...
        S_unstable_Best_Best{ii}(3,1:ind_unstable_best_Best{ii}),...
        'Color',colors,...
        'LineWidth',3)

    plot3(S_stable_Best_Best{ii}(1,1:ind_stable_best_Best{ii}),...
        S_stable_Best_Best{ii}(2,1:ind_stable_best_Best{ii}),...
        S_stable_Best_Best{ii}(3,1:ind_stable_best_Best{ii}),'r',...
        'Color',colors,...
        'LineWidth',3)
    plot3(S_stable_Best_Best{ii}(1,ind_stable_best_Best{ii}),...
        S_stable_Best_Best{ii}(2,ind_stable_best_Best{ii}),...
        S_stable_Best_Best{ii}(3,ind_stable_best_Best{ii}),'rd',...
        'Color',colors,...
        'MarkerFaceColor','r',...
        'MarkerSize',9)
end
xlabel('\chi_x')
ylabel('\chi_y')
ax = gca; % current axes
ax.FontSize = 30;

[X,Y] = meshgrid(-1.5:1.5);
pic = plane_eq_Draw(X,Y);
surface(X,Y,pic)
xlim([-1.5 1.5])
ylim([-1.5 1.5])
zlim([-1.5 1.5])

output.S_Wu_best = S_unstable_Best_Best;
output.t_Wu_best = t_unstable_Best_Best;
output.S_Ws_best = S_stable_Best_Best;
output.t_Ws_best = t_stable_Best_Best;
output.Wu_best_ind = ind_unstable_best_Best;
output.Ws_best_ind = ind_stable_best_Best;

end